#include "Spring.h"

#ifndef __Particle_h__
  #include "Particle.h"
#endif //__Particle_h__

Spring::Spring(Particle *p1, Particle *p2) :
	m_pParticle1(p1),
	m_pParticle2(p2)
{
	vec3 vec = p1->getPosition()-p2->getPosition();
	m_nRestDistance = length(vec);
}

// This function applies spring on the two connected particles to force them back to the rest distance.
void Spring::applySpring()
{
	// Calculating a vector from p1 to p2
	vec3 p1_to_p2 = m_pParticle2->getPosition() - m_pParticle1->getPosition();

	// Getting the current distance between p1 and p2
	float current_distance = length(p1_to_p2);

	// Calculating the offset vector which can move p1 into the rest distance to p2
	vec3 correctionVector = p1_to_p2*(1.0f - m_nRestDistance/current_distance); 

	// Making the correction vector a half of the calculated length for p1, 
	// so that the other half will be applied for p2
	vec3 correctionVectorHalf = correctionVector*0.5f;

	// Moving p1 the half of the correction vector
	m_pParticle1->offsetPosition(correctionVectorHalf);

	// Moving p2 the half of the correction vector in the negativce direction
	m_pParticle2->offsetPosition(-correctionVectorHalf);
}